program twiss_ptc
use madx_keywords
implicit none
real(dp) x_orbit(6)
integer i,NO
type(fibre), pointer :: p
type(layout), pointer :: my_ring
REAL(DP) BETA_X,BETA_Z
TYPE(REAL_8) X_POL(6)
TYPE(DAMAP) ID
TYPE(NORMALFORM) NFORM
TYPE(mad_universe) m_u

call set_up_universe(m_u)
call APPEND_EMPTY_LAYOUT(m_u)
call READ_INTO_VIRGIN_LAYOUT(m_u%start,"als.txt")  

MY_RING=>m_u%start
p=>my_ring%start

CALL ALLOC(X_POL);

x_orbit=0.d0; x_orbit(5)=0.01d0;
 call find_orbit(my_ring,x_orbit,1,default,1.d-6)
write(6,*) x_orbit

WRITE(6,*) "GIVE ORDER OF TAYLOR MAP ";READ(5,*) NO;

IF(NO>0) THEN
 CALL INIT(DEFAULT,NO,0,BERZ)
 CALL ALLOC(ID);CALL ALLOC(NFORM);
 ID=1
 X_POL=X_ORBIT+ID
ELSE
 X_POL=X_ORBIT
ENDIF


 call track(my_ring,X_POL,1,DEFAULT)

WRITE(6,*) "POLYMORPHS PRINTED "
CALL PRINT(X_POL,6)


IF(NO>0) THEN   ! WE WILL DO A TWISS CALCULATION OF BETA_X

 NFORM=X_POL

 WRITE(6,*) NFORM%TUNE

 X_POL=X_ORBIT+NFORM%A_T

 BETA_X= (X_POL(1).SUB.'100000')**2+(X_POL(1).SUB.'010000')**2
 BETA_Z= (X_POL(6).SUB.'000010')**2+(X_POL(6).SUB.'000001')**2


  WRITE(6,*) 'POS = ',0,'            BETA_X = ', BETA_X
  WRITE(6,*) '                               BETA_Z = ', BETA_Z
 DO I=1,my_ring%n
 
  call track(my_ring,X_POL,i,i+1,DEFAULT)  
 BETA_X= (X_POL(1).SUB.'100000')**2+(X_POL(1).SUB.'010000')**2
 BETA_Z= (X_POL(6).SUB.'000010')**2+(X_POL(6).SUB.'000001')**2
 
 IF(MOD(I,50)==1.OR.I==my_ring%n) then
  WRITE(6,*) 'POS = ',I,'            BETA_X = ', BETA_X
  WRITE(6,*) '                               BETA_Z = ', BETA_Z
 ENDIF

 ENDDO

ENDIF

CALL KILL(X_POL);
IF(NO>1) THEN
 CALL KILL(ID);
ENDIF

end program twiss_ptc


